//
// Created by 74354 on 2022/1/4.
//

#ifndef MYDH_ROBOT_H
#define MYDH_ROBOT_H

#include <iostream>
#include <vector>
#include "armadillo"

using namespace std;
using namespace arma;

#define PI 3.1415926535897932384626

// Armadillo在线文档在这里:
// http://arma.sourceforge.net/docs.html



/*

 */
class Robot;
class Joint;

typedef enum JointType{
    Rotate = 0,
    Line = 1
}JointType_t;

typedef vector<float> JointPos;
JointPos createJointValue(mat pos,int count);


typedef struct{
    float pX;
    float pY;
    float pZ;
    float rX;
    float rY;
    float rZ;
}DescartesPos;

class Robot {
public:
    Robot(){}
    mat T_froward{4,4,fill::eye};
    mat T_backward;
    mat DH_Mat;
    double moveStep;           // [cm]
    double rotateStep;         // [rad]
    vector<Joint*> Joints;

    mat getTheta(){return DH_Mat(span(),3);}
    bool checkTheta(mat thetaMat);
    mat fkine(mat thetaMat);
    mat ikine(mat posMat);
    mat getDelta(mat endPos); //获取微分算子
    mat jacobian();
    void appendJoint(JointType_t type,double a,double alpha,double d,double theta);
    void appendJoint(Joint* joint);
    void removeAllJoints();
};



class Joint{
public:
    JointType_t type;
    double theta;
    double d;
    double alpha;
    double a;
    double theta_min;
    double theta_max;

    mat T;
    Joint(){}

    Joint(double a,double alpha,double d,double theta):
    theta(theta),d(d),alpha(alpha),a(a){
        transfer();
    }
    Joint(JointType typ,double a,double alpha,double d,double theta):
    type(typ),theta(theta),d(d),alpha(alpha),a(a){
        transfer();}

    Joint(JointType typ,double a,double alpha,double d,double theta,double the_min,double the_max):\
        type(typ),theta(theta),d(d),alpha(alpha),a(a),theta_min(the_min),theta_max(the_max){
        transfer();
    }
    void config(JointType typ,double _a,double alp,double _d,double the){
        theta = the;a = _a;alpha = alp;d = _d;type = typ;
        transfer();
    }

    ~Joint(){}

    void set_theta(double value);
    void set_d(double value);
    void transfer();

};



#endif //MYDH_ROBOT_H
